#include "CFormation.h"
#include <math.h>

#define PI 3.14159265

CFormation::TFormationPairs CFormation::formationPairs;
std::vector<CFormation*> CFormation::formations;

CFormation::CFormation(BWAPI::Position pos, EFormations form)
:formation(form), target(0)
{
	setCenter(pos);
	formations.push_back(this);
}

CFormation::~CFormation()
{
	disbandFormation();
	for (int i = 0; i < formations.size(); ++i)
	{
		if (formations[i] == this)
		{
			for (int j = i + 1; j < formations.size(); ++j)
			{
				formations[j-1] = formations[j];
			}
			formations.pop_back();
			i = formations.size();
		}
	}
}

void CFormation::disbandFormation()
{
	TFormationPairs::const_iterator iter = formationPairs.begin();
	for (; iter != formationPairs.end(); ++iter)
	{
		if (iter->second == this)
			formationPairs.erase(iter);
	}
	units.clear();
}

void CFormation::splitFormation()
{
	setCenter(getCenter() + BWAPI::Position(100,0));
	CFormation* newGroup = getEmptyFormation(getCenter() + BWAPI::Position(-100,0));
	int count = units.size() >> 1;
	for (int i = 0; i < count; ++i)
	{
		TUnitMap::const_iterator iter = units.begin();
		newGroup->joinFormation(iter->first);
	}
}

void CFormation::setFormation(EFormations f)
{
	formation = f;
	recalculateSlots();
}

BWAPI::Position CFormation::getCenter()
{
	static bool testing;
	if (!testing)
	{
		testing = true;
		if (waypoints.size() > 1 && isWellFormed())
			waypoints.pop_front();
		testing = false;
	}
	return waypoints.front();
}

void CFormation::setCenter(BWAPI::Position pos)
{
	waypoints.clear();
	waypoints.push_back(pos);
}

void CFormation::addWayPoint(BWAPI::Position pos)
{
	waypoints.push_back(pos);
}

BWAPI::Unit* CFormation::getTarget()
{
	if (target && target->getHitPoints() == 0)
		target = 0;
	return target;
}

void CFormation::setTarget(BWAPI::Unit* t)
{
	target = t;
}

CFormation::TUnitMap CFormation::getUnits()
{
	return units;
}

int CFormation::getUnitSlot(BWAPI::Unit* unit)
{
	TUnitMap::const_iterator iter = units.find(unit);
	if (iter != units.end())
		return iter->second;
	return 0;
}

void CFormation::joinFormation(BWAPI::Unit* unit)
{
	if (units.size() == 0)
		target = 0;
	if (CFormation* old = getFormation(unit))
		old->leaveFormation(unit);
	formationPairs.insert(std::make_pair(unit, this));
	units.insert(std::make_pair(unit, units.size()));
	recalculateSlots();
	calcSlotSize();
}

void CFormation::leaveFormation(BWAPI::Unit* unit)
{
	formationPairs.erase(unit);
	units.erase(unit);
	recalculateSlots();
	calcSlotSize();
}

void CFormation::recalculateSlots()
{
	std::map<int, BWAPI::Position> slots;
	for (int i = 0; i < units.size(); ++i)
		slots.insert(std::make_pair(i, getPosition(i)));
	CalculateNearestSlots(units, slots);
}

CFormation* CFormation::getFormation(BWAPI::Unit* unit)
{
	TFormationPairs::const_iterator iter = formationPairs.find(unit);
	if (iter != formationPairs.end())
		return iter->second;
	return 0;
}

bool CFormation::isWellFormed()
{
	for (TUnitMap::iterator iter = units.begin(); iter != units.end(); ++iter)
		if (!isInPosition(iter->first))
			return false;
	return true;
}

bool CFormation::isInPosition(BWAPI::Unit* unit)
{
	BWAPI::Position pos = getPosition(unit);
	if (!(unit->getType().isFlyer()))
		if (unit->getTilePosition().isValid())
			if (!BWTA::isConnected(unit->getTilePosition(), BWAPI::TilePosition(pos)))
				return true;
	return (pos == unit->getPosition());
}

void CFormation::calcSlotSize()
{
	int size = 2;

	for (TUnitMap::const_iterator iter = units.begin(); iter != units.end(); ++iter)
	{
			if (iter->first->getType().spaceRequired() < 10 && iter->first->getType().spaceRequired() > size)
				size = iter->first->getType().spaceRequired();
	}
	slotSize = size*10 + 5;
}

BWAPI::Position CFormation::getPosition(BWAPI::Unit* unit)
{
	return getPosition(getUnitSlot(unit));
}

BWAPI::Position CFormation::getPosition(int slot)
{
	if (units.size() <= 1)
		return getCenter();
	switch (formation)
	{
	case ZFVLine:
		return getCenter() + BWAPI::Position(0, slot * slotSize - slotSize/2 * units.size());
	case ZFHLine:
		return getCenter() + BWAPI::Position(slot * slotSize - slotSize/2 * units.size(), 0);
	case ZFForeslash:
		return getCenter() + BWAPI::Position(-slot * slotSize + slotSize/2 * units.size(), slot * slotSize - slotSize/2 * units.size());
	case ZFBackslash:
		return getCenter() + BWAPI::Position(slot * slotSize - slotSize/2 * units.size(), slot * slotSize - slotSize/2 * units.size());
	case ZFDoubleVLine:
		if (slot%2 == 0)
			return getCenter() + BWAPI::Position(0, slot * slotSize - slotSize/2 * units.size());
		return getCenter() + BWAPI::Position(slotSize, slot * slotSize - slotSize/2 * units.size() - slotSize);
	case ZFDoubleVLineDense:
		if (slot%2 == 0)
			return getCenter() + BWAPI::Position(0, slot * slotSize/2 - slotSize/4 * units.size());
		return getCenter() + BWAPI::Position(slotSize, slot * slotSize/2 - slotSize/4 * units.size() - slotSize/2);
	case ZFDoubleHLine:
		if (slot%2 == 0)
			return getCenter() + BWAPI::Position(slot * slotSize - slotSize/2 * units.size(), 0);
		return getCenter() + BWAPI::Position(slot * slotSize - slotSize/2 * units.size() - slotSize, slotSize);
	case ZFDoubleHLineDense:
		if (slot%2 == 0)
			return getCenter() + BWAPI::Position(slot * slotSize/2 - slotSize/4 * units.size(), 0);
		return getCenter() + BWAPI::Position(slot * slotSize/2 - slotSize/4 * units.size() - slotSize/2, slotSize);
	case ZFCircle:
		{
			int circum = slotSize*(units.size()+1);
			int radius = ((radius = circum/2/PI) > slotSize ? radius : slotSize);
			double angle = 360.0*(slot)/units.size();
			double s = sin(angle * PI/180.0);
			double c = cos(angle * PI/180.0);
			return getCenter() + BWAPI::Position( radius*s, radius*c );
		}
	case ZFCross:
		if (( slot )%4 == 0)
			return getCenter() + BWAPI::Position( slotSize*(( slot )/4 + (units.size()%2 ? 0 : 1)), slotSize*(( slot )/4 + (units.size()%2 ? 0 : 1)));
		if ((slot+3)%4 == 0)
			return getCenter() + BWAPI::Position( slotSize*((slot+1)/4), -slotSize*((slot+1)/4));
		if ((slot+2)%4 == 0)
			return getCenter() + BWAPI::Position(-slotSize*((slot+2)/4),  slotSize*((slot+2)/4));
		if ((slot+1)%4 == 0)
			return getCenter() + BWAPI::Position(-slotSize*((slot+3)/4), -slotSize*((slot+3)/4));
	case ZFPlus:
		if (( slot )%4 == 0)
			return getCenter() + BWAPI::Position( +slotSize*(( slot )/4+1),  0);
		if ((slot+1)%4 == 0)
			return getCenter() + BWAPI::Position( -slotSize*((slot+1)/4), 0);
		if ((slot+2)%4 == 0)
			return getCenter() + BWAPI::Position(0,  slotSize*((slot+2)/4));
		if ((slot+3)%4 == 0)
			return getCenter() + BWAPI::Position(0, -slotSize*((slot+3)/4));
	case ZFDiamond:
		{
			int circum = slotSize*(units.size()+1);
			int radius = 2 * ((radius = circum/2/PI) > slotSize ? radius : slotSize);
			double angle = 360.0*(slot)/units.size();
			double s = sin(angle * PI/180.0) / (abs(cos(angle * PI/180.0)) + 1);
			double c = cos(angle * PI/180.0) / (abs(sin(angle * PI/180.0)) + 1);
			return getCenter() + BWAPI::Position( radius*s, radius*c );
		}
	case ZFBox:
		{
			int width = 1 + ceil(((double)units.size())/4);
			BWAPI::Position topLeft = getCenter() + BWAPI::Position(-slotSize/2*(width-1), -slotSize/2*(width-1));
			BWAPI::Position bottomRight = getCenter() + BWAPI::Position(slotSize/2*(width-1), slotSize/2*(width-1));
			int mid = (width-1)*2;
			if (slot == 0)
				return topLeft;
			if (slot < width)
				return topLeft + BWAPI::Position(slotSize*slot, 0);
			if (slot < mid)
				return bottomRight - BWAPI::Position(0, slotSize*(mid-slot));
			if (slot == mid)
				return bottomRight;
			if ((slot > mid) && (slot <= units.size()-width))
				return bottomRight + BWAPI::Position(slotSize*(mid-slot), 0);
			return topLeft + BWAPI::Position(0, slotSize*(units.size() - slot));
		}
	case ZFBlock:
		{
			int width = floor(sqrt((double)units.size()));
			int height = ceil((double)units.size()/width);
			int x = slot%width;
			int y = floor((double)slot/width);
			return getCenter() + BWAPI::Position(slotSize*x - slotSize/2 * width, slotSize*y - slotSize/2 * height);
		}
		//case ZFHourGlassV: //Test
		//      {
		//              if (units.size() == 0)
		//                      return center;
		//              int circum = slotSize*(units.size()+2);
		//              int radius = 2 * ((radius = circum/2/PI) > slotSize ? radius : slotSize);
		//              double angle = 360.0*(slot)/units.size();
		//              double s = sin(angle * PI/180.0) / (abs(tan(angle * PI/180.0)) + 1);
		//              double c = cos(angle * PI/180.0) / (abs(tan(angle * PI/180.0)) + 1);
		//              return center + BWAPI::Position( radius*s, radius*c );
		//      }
		//case ZFHourGlassH: //Test
		//      {
		//              if (units.size() == 0)
		//                      return center;
		//              int circum = slotSize*(units.size()+1);
		//              int radius = 2 * ((radius = circum/2/PI) > slotSize ? radius : slotSize);
		//              double angle = 360.0*(slot)/units.size();
		//              double s = sin(angle * PI/180.0) / (abs(tan(angle * PI/180.0)) + 1);
		//              double c = cos(angle * PI/180.0) / (abs(tan(angle * PI/180.0)) + 1);
		//              return center + BWAPI::Position( radius*c, radius*s );
		//      }
	case ZFNone:
		return getCenter();
	}
	return getCenter();
}


std::vector<CFormation*> CFormation::getFormations()
{
	std::vector<CFormation*> local;
	for (int i = 0; i < formations.size(); ++i)
	{
		if (formations[i]->units.size() > 0)
			local.push_back(formations[i]);
	}
	return local;
}

CFormation* CFormation::getEmptyFormation(BWAPI::Position center, EFormations formation)
{
	for (int i = 0; i < formations.size(); ++i)
		if (formations[i]->units.size() == 0)
		{
			formations[i]->setCenter(center);
			formations[i]->setFormation(formation);
			return formations[i];
		}
		return new CFormation(center, formation);
}

void CalculateNearestSlots(CFormation::TUnitMap& units, std::map<int, BWAPI::Position> slots)
{
	typedef std::map<BWAPI::Unit*, std::pair<int, BWAPI::Position> > TMatch;

	CFormation::TUnitMap::iterator unitIter = units.begin();
	TMatch matches;
	std::map<int, BWAPI::Position>::const_iterator slotIter = slots.begin();

	for (; unitIter != units.end(); ++unitIter)
	{
		//Get closest unclaimed position
		std::map<int, BWAPI::Position>::const_iterator closest = slots.begin();
		if (closest != slots.end())
		{
			double dist = unitIter->first->getPosition().getDistance(closest->second);
			for (; slotIter != slots.end(); ++slotIter)
			{
				if (double next = unitIter->first->getPosition().getDistance(closest->second) < dist)
				{
					dist = next;
					closest = slotIter;
				}
			}
			unitIter->second = closest->first;
			matches.insert(std::make_pair(unitIter->first, (*closest)));
			slots.erase(closest);
		}
	}
	//seek trades
	for (TMatch::iterator iter = matches.begin(); iter != matches.end(); ++iter)
	{
		for (TMatch::iterator iter1 = matches.begin(); iter1 != matches.end(); ++iter1)
		{
			for (TMatch::iterator  iter2 = matches.begin(); iter2 != matches.end(); ++iter2)
			{
				double mineMine = iter1->first->getPosition().getDistance(iter1->second.second);
				double mineOther = iter1->first->getPosition().getDistance(iter2->second.second);
				double otherMine = iter2->first->getPosition().getDistance(iter1->second.second);

				if ((otherMine <= mineMine) && (mineOther <= mineMine)) //trade
				{
					std::pair<int, BWAPI::Position> otherSlot = iter2->second;
					iter2->second = iter1->second;
					iter1->second = otherSlot;
					if ((unitIter = units.find(iter1->first)) != units.end())
						unitIter->second = iter1->second.first;
					if ((unitIter = units.find(iter2->first)) != units.end())
						unitIter->second = iter2->second.first;
				}
			}
		}
	}
}

//for (; slotIter != slots.end(); ++slotIter)
//{
//      //Get closest unclaimed unit
//      CFormation::TUnitMap::iterator closest = units.begin();
//      if (closest != units.end())
//      {
//              double dist = slotIter->second.getDistance(closest->first->getPosition());
//              for (; unitIter != units.end(); ++unitIter)
//              {
//                      if (double next = slotIter->second.getDistance(closest->first->getPosition()) < dist)
//                      {
//                              dist = next;
//                              closest = unitIter;
//                      }
//              }
//              unitIter->second = closest->first;
//      }
//}

